/**
 * @(#) RobotBrain.java
 */

package pcsr.simulator.control;

import java.awt.Rectangle;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Hashtable;
import java.util.LinkedList;
import java.util.List;
import java.util.Properties;
import java.util.concurrent.atomic.AtomicBoolean;

import pcsr.drivers.simulatorDriver.MotorActuator;
import pcsr.drivers.simulatorDriver.ProximitySensor;
import pcsr.drivers.simulatorDriver.SimulatorActuator;
import pcsr.drivers.simulatorDriver.SimulatorSensor;


public class RobotBrain extends Thread implements ISimulatorObservable, ISimulatorActions, ISimulatorStatusObservable
{
	private static RobotBrain instance;

	public AtomicBoolean run = new AtomicBoolean();

	private Hashtable<String, ControlSimulatorActuator> actuators;

	private Hashtable<String, ControlSimulatorEmitter> fieldEmitters;

	private int refreshRate;

	private ControlSimulatorStructure robotStructure;

	private Hashtable<String, ControlSimulatorSensor> sensors;

	private List<ISimulatorObserver> simulatorObservers;

	private ArrayList<ISimulatorStatusObserver> simulatorStatusObservers = new ArrayList<ISimulatorStatusObserver>();

	private Hashtable<String, ControlSimulatorWall> walls;

	public static RobotBrain GetInstance()
	{
		if(instance == null)
		{
			instance = new RobotBrain(1);
		}
		return instance;
	}

	/**
	 * @return the robotStructure
	 */
	public ControlSimulatorStructure GetRobotStructure() {
		return robotStructure;
	}

	@Override
	public void ObserveSimulator(ISimulatorObserver observer) {
		simulatorObservers.add(observer);
	}

	@Override
	public void ObserveSimulatorStatus(ISimulatorStatusObserver observer) {
		simulatorStatusObservers.add(observer);
	}

	public void run() {
		NotifySimulatorWallsObservers();
		NotifySimulatorEmittersObservers();
		while (run.get()) {
			try {
				sleep(refreshRate);
			} 
			catch (InterruptedException e) {
			}

			boolean robotCollided = false;

			ControlSimulatorStructure aheadStateRobot = null;
			try {
				aheadStateRobot = (ControlSimulatorStructure) GetRobotStructure().clone();
			} catch (CloneNotSupportedException e) {
				e.printStackTrace();
				return;
			}
			aheadStateRobot.Move();

			// Check if robot structure will collide with a wall
			robotCollided = CollisionWallRobot(robotCollided, aheadStateRobot);
			
			if(!robotCollided) 
			{
				ControlSimulatorStructure _bot = GetRobotStructure();
				_bot.setXPos(aheadStateRobot.GetXPos());
				_bot.setYPos(aheadStateRobot.GetYPos());
				_bot.setXSpeed(aheadStateRobot.getXSpeed());
				_bot.setYSpeed(aheadStateRobot.getYSpeed());
				_bot.robotAngle = aheadStateRobot.robotAngle;

				NotifySimulatorRobotObservers();
			}

			
			// Check for the value of sensors against the walls.
			CollisionWallSensor();
			
			// Check for the value of sensors against the field emitters.
			CollisionEmitterSensor();
			
			for (ControlSimulatorSensor s : sensors.values()) {
				UpdateSimulatorSensorValue(s, s.getSensorValue());
				s.resetSensorIntersect();
			}

		}
	}

	/**
	 * @param robotCollided
	 * @param aheadStateRobot
	 * @return
	 */
	private boolean CollisionWallRobot(boolean robotCollided,
			ControlSimulatorStructure aheadStateRobot) {
		for(ControlSimulatorWall wall : walls.values())
		{
			if(wall.CheckCollision(aheadStateRobot))
			{
				robotCollided = true;
			}
		}
		return robotCollided;
	}

	/**
	 * 
	 */
	private void CollisionEmitterSensor() {
		for(ControlSimulatorEmitter emitter : fieldEmitters.values())
		{
			for(ControlSimulatorSensor sensor : sensors.values())
			{
				if(emitter.CheckCollision(sensor))
				{
					System.err.println("Emitter colided with sensor");
					//sensor.ProcessCollision(emitter);
				}
			}
		}
	}

	/**
	 * 
	 */
	private void CollisionWallSensor() {
		for(ControlSimulatorWall wall : walls.values())
		{	
			for(ControlSimulatorSensor sensor : sensors.values())
			{
				if(wall.CheckCollision(sensor))
				{
					Rectangle collissionArea = wall.GetCollisionArea(sensor);
					//sensor.setSensorIntersect(collissionArea.getWidth(), collissionArea.getHeight());
					sensor.setSensorIntersect(collissionArea);
				}
			}
		}
	}

	@Override
	public void SetSimulatorActuatorMode(String actuatorID, String actuatorMode) {
		actuators.get(actuatorID).setMode(actuatorMode);
	}

	@Override
	public void SetSimulatorActuatorValue(String actuatorID,
			double actuatorValue) {
		actuators.get(actuatorID).setActuatorValue(actuatorValue);
	}

	@Override
	public void SetSimulatorSensorMode(String sensorID, String sensorMode) {
		sensors.get(sensorID).setMode(sensorMode);
	}

	@Override
	public void SetSimulatorSensorValue(String sensorID, double sensorValue) {
		// TODO Auto-generated method stub

	}

	@Override
	public void UnObserveSimulator(ISimulatorObserver observer) {
		simulatorObservers.remove(observer);
	}

	@Override
	public void UnObserveSimulatorStatus(ISimulatorStatusObserver observer) {
		simulatorStatusObservers.remove(observer);
	}

	/**
	 * Create a map of walls and field emitters to interact with the robot
	 * @param wallWidth the width for each wall
	 * @param wallHeight the height for each wall
	 */
	private void CreateFieldMap(int wallWidth, int wallHeight) {
		for(int top = 1; top < 40; top ++)
		{
			ControlSimulatorWall wall = new ControlSimulatorWall("wallTop"+top);			
			wall.setXPos(top*wallWidth);
			wall.setYPos(0);
			wall.setWidth(wallWidth);
			wall.setHeight(wallHeight);
			walls.put(wall.GetID(), wall);
		}
		
		for(int top2 = 10; top2 < 30; top2 ++)
		{
			ControlSimulatorWall wall = new ControlSimulatorWall("wallTop2"+top2);			
			wall.setXPos(top2*wallWidth);
			wall.setYPos(110);
			wall.setWidth(wallWidth);
			wall.setHeight(wallHeight);
			walls.put(wall.GetID(), wall);
		}
		
		for(int bottom = 1; bottom < 40; bottom ++)
		{
			ControlSimulatorWall wall = new ControlSimulatorWall("wallBottom"+bottom);			
			wall.setXPos(bottom*wallWidth);
			wall.setYPos(390);
			wall.setWidth(wallWidth);
			wall.setHeight(wallHeight);
			walls.put(wall.GetID(), wall);
		}
		
		for(int right = 1; right < 40; right ++)
		{
			ControlSimulatorWall wall = new ControlSimulatorWall("wallRight"+right);			
			wall.setXPos(390);
			wall.setYPos(right*wallHeight);
			wall.setWidth(wallWidth);
			wall.setHeight(wallHeight);
			walls.put(wall.GetID(), wall);
		}
		
		for(int left = 0; left < 40; left ++)
		{
			ControlSimulatorWall wall = new ControlSimulatorWall("wallLeft"+left);			
			wall.setXPos(0);
			wall.setYPos(left*wallHeight);
			wall.setWidth(wallWidth);
			wall.setHeight(wallHeight);
			walls.put(wall.GetID(), wall);
		}
		
		for(int left2 = 20; left2 < 30; left2 ++)
		{
			ControlSimulatorWall wall = new ControlSimulatorWall("wallLeft2"+left2);			
			wall.setXPos(60);
			wall.setYPos(left2*wallHeight);
			wall.setWidth(wallWidth);
			wall.setHeight(wallHeight);
			walls.put(wall.GetID(), wall);
		}
		/* */
		ControlSimulatorEmitter emitter = new ControlSimulatorEmitter("emitter");
		emitter.setXPos(230);
		emitter.setYPos(90);
		emitter.setWidth(50);
		emitter.setHeight(50);
		fieldEmitters.put(emitter.GetID(), emitter);
		
		ControlSimulatorEmitter emitter2 = new ControlSimulatorEmitter("emitter2");
		emitter2.setXPos(200);
		emitter2.setYPos(200);
		emitter2.setWidth(50);
		emitter2.setHeight(50);
		fieldEmitters.put(emitter2.GetID(), emitter2);
		/* */
	}

	/**
	 * Initializes the robot.
	 * Defines the wheels and sensors for the robot.
	 * Defines default values for wheels.
	 */
	private void InitializeRobot() {
				
		SimulatorRobotFactory.createInstance(this);
		
		ControlSimulatorActuator rightWheel = new ControlSimulatorActuator("rightWheel",robotStructure);
		rightWheel.setXDeltaPosition(robotStructure.GetWidth());
		rightWheel.setYDeltaPosition(robotStructure.GetHeight()/2);
		robotStructure.setRightWheel(rightWheel);
		actuators.put(rightWheel.GetID(), rightWheel);
		
		ControlSimulatorActuator leftWheel = new ControlSimulatorActuator("leftWheel",robotStructure);
		leftWheel.setXDeltaPosition(0);
		leftWheel.setYDeltaPosition(robotStructure.GetHeight()/2);
		robotStructure.setLeftWheel(leftWheel);
		actuators.put(leftWheel.GetID(), leftWheel);
		
	}

	/**
	 * @param rightSensor
	 */
	void AddNewSensorToStructure(ControlSimulatorSensor rightSensor) {
		robotStructure.addSensors(rightSensor);
		sensors.put(rightSensor.GetID(), rightSensor);
	}

	private void NotifySimulatorRobotObservers() {
		for(ISimulatorStatusObserver sso : simulatorStatusObservers)
		{
			sso.NotifyRobotChange(GetRobotStructure());
		}
	}

	private void NotifySimulatorWallsObservers() {
		for(ISimulatorStatusObserver sso : simulatorStatusObservers)
		{			
			sso.NotifyWallsChange(walls);
		}
	}
	
	private void NotifySimulatorEmittersObservers() {
		for(ISimulatorStatusObserver sso : simulatorStatusObservers)
		{			
			sso.NotifyEmittersChange(fieldEmitters);
		}
	}

	void UpdateSimulatorActuatorValue(ControlSimulatorActuator actuator, double newValue)
	{
		for(ISimulatorObserver so : simulatorObservers)
		{
			so.NotifySimulatorActuatorValue(actuator.GetID(), newValue);
		}
	}

	void UpdateSimulatorSensorValue(ControlSimulatorSensor sensor, double newValue)
	{
		for(ISimulatorObserver so : simulatorObservers)
		{
			so.NotifySimulatorSensorValue(sensor.GetID(), newValue);
		}
	}

	{
		actuators = new Hashtable<String, ControlSimulatorActuator>();
		fieldEmitters = new Hashtable<String, ControlSimulatorEmitter>();
		sensors = new Hashtable<String, ControlSimulatorSensor>();		
		walls = new Hashtable<String, ControlSimulatorWall>();
		
		simulatorObservers = new LinkedList<ISimulatorObserver>();
		
		robotStructure = new ControlSimulatorStructure("SimulatorStructure");		
	}

	private RobotBrain(int refreshInSeconds)
	{
		refreshRate = refreshInSeconds * 1000;
		int wallWidth = 10;
		int wallHeight = 10;
		
		InitializeRobot();

		CreateFieldMap(wallWidth, wallHeight);		
	}

	@Override
	public void Start() {
		run.set(true);
		this.start();
	}

	@Override
	public Collection<SimulatorActuator> GetAllActuators() {
		Collection<SimulatorActuator> ret = new LinkedList<SimulatorActuator>();
		
		for(ControlSimulatorActuator csa : actuators.values())
		{
			SimulatorActuator sa = new MotorActuator(csa.GetID(),"NONE", new String[]{"NONE"});
			sa.SetActuatorValue(csa.getActuatorValue());
			ret.add(sa);
		}
		
		return ret;
	}

	@Override
	public Collection<SimulatorSensor> GetAllSensors() {
		Collection<SimulatorSensor> ret = new LinkedList<SimulatorSensor>();
		
		for(ControlSimulatorSensor css : sensors.values())
		{
			SimulatorSensor ss = new ProximitySensor(css.GetID(),"PROX",new String[]{"PROX"});
			ss.SetSensorValue(css.getSensorValue());
			ret.add(ss);
		}
		
		return ret;
	}	
}

class SimulatorRobotFactory
{
	private static final String PREFIX = "pcsr.simulator.robot";
	private static final String ROBOT_NAME = PREFIX+".name";
	private static final String ROBOT_WIDTH = PREFIX+".width";
	private static final String ROBOT_HEIGHT = PREFIX+".height";
	private static final String ROBOT_XPOS = PREFIX+".xpos";
	private static final String ROBOT_YPOS = PREFIX+".ypos";
	private static final String ROBOT_ANGLE = PREFIX+".angle";
	private static final String ROBOT_SENSORCOUNT = PREFIX+".sensorCount";
	private static final String ROBOT_SENSORPREFIX = PREFIX+".sensor";	
	
	private static final String PROPSFNAME = "PCSR_Simulator.properties";
	private static final String UDIR = System.getProperty("user.dir");
	private static final String UHOME = System.getProperty("user.home");
	private static final String FSEP = System.getProperty("file.separator");
	
	public static void createInstance( RobotBrain bot) 
	{
		
		/* Create properties with defaults for appropriate OS */
		Properties props = getDefaults();

		/* Load settings from PCSR.properties file */
		File file = new File(UDIR + FSEP + PROPSFNAME);
		if(!file.exists())
			file = new File(UHOME + FSEP + PROPSFNAME);
		System.out.println("Properties location: " + file.getAbsolutePath());
		try {
			if(file.exists()) 
				props.load(new FileInputStream(file));
			else {
				System.out.println(PROPSFNAME + " does NOT exist. Creating.");

				file.createNewFile();
				// NOTE: Default settings might not be stored?
				props.store(new FileOutputStream(file), "Auto-generated PCSR Simulator Properties file");
			}

		} catch(IOException e) {
			System.out.println(e);
		}

		String simulatorrobotname = props.getProperty(SimulatorRobotFactory.ROBOT_NAME);
		String simulatorrobotwidth = props.getProperty(SimulatorRobotFactory.ROBOT_WIDTH);
		String simulatorrobotheight = props.getProperty(SimulatorRobotFactory.ROBOT_HEIGHT);
		String simulatorrobotxpos = props.getProperty(SimulatorRobotFactory.ROBOT_XPOS);
		String simulatorrobotypos = props.getProperty(SimulatorRobotFactory.ROBOT_YPOS);
		String simulatorrobotangle = props.getProperty(SimulatorRobotFactory.ROBOT_ANGLE);
		if (simulatorrobotname != null) 
		{	
			// TODO: Set Robot name here
			bot.GetRobotStructure().setWidth(Integer.valueOf(simulatorrobotwidth));
			bot.GetRobotStructure().setHeight(Integer.valueOf(simulatorrobotheight));
			bot.GetRobotStructure().setXPos(Integer.valueOf(simulatorrobotxpos));
			bot.GetRobotStructure().setYPos(Integer.valueOf(simulatorrobotypos));
			bot.GetRobotStructure().setRobotAngle(Double.valueOf(simulatorrobotangle));
		}
		
		String sensorCount = props.getProperty(SimulatorRobotFactory.ROBOT_SENSORCOUNT);
		int iSensorCount = Integer.valueOf(sensorCount);
		
		for(int idx = 1; idx <= iSensorCount; idx++)
		{
			String sensorConfig = props.getProperty(SimulatorRobotFactory.ROBOT_SENSORPREFIX+idx);
			String[] sensorConfigProps = sensorConfig.split("\\|");
			//<sensorName>|<sensorDeltaX>|<sensorDeltaY>|<Range>|<angle>
			ControlSimulatorSensor tempSensor = new ControlSimulatorSensor(sensorConfigProps[0],bot.GetRobotStructure(), Double.valueOf(sensorConfigProps[4]) );
			tempSensor.setXDeltaPosition(bot.GetRobotStructure().GetWidth()/2);
			tempSensor.setYDeltaPosition(bot.GetRobotStructure().GetHeight()/2);
			tempSensor.setSensorRange(Integer.valueOf(sensorConfigProps[3]));
			bot.AddNewSensorToStructure(tempSensor);
		}

	}

	/**
	 * This creates a Properties object with some default settings.
	 * The Properties class will resort to using default settings
	 * if a setting is blank.
	 * @return Properties object with some default settings
	 */
	private static Properties getDefaults() {
		Properties defaults = new Properties();
		
		defaults.setProperty(SimulatorRobotFactory.ROBOT_NAME, "PCSR Simulator Robot");
		
		return defaults;
	}
}
